void init_mroute()    // initialize structures
{
  routebuffer_size = 0;
  memset(route_buffer, 0, sizeof(route_buffer));
  route_seq = 0;

  memset(payload_buffer, 0, sizeof(payload_buffer));
  payloadbuffer_first = 0; payloadbuffer_last = 0; payloadbuffer_size = 0;
}

void handle_msghi(ADDRESS source_node, ADDRESS dest_node, uint8_t path_nodecount,
  ADDRESS *path_nodes, uint8_t data_len, uint8_t *data)  // main handler for payloads reaching destination
{
#ifdef TOSSIM
  if (config.disable_msglog == 0)
    log_rcvcmdhi(data, data_len);
#endif

  DBGOUT("Received forwarding from node %d", source_node);
  if (path_nodecount > 0)
    print_path(path_nodes, path_nodecount, "Path");

  if (data_len > 0)
    switch (data[0] & CMDHI_MASK)
    {
      case CMDHI_INIT:                 handle_initmsghi(source_node, dest_node, path_nodecount, path_nodes, data_len, data); break;
      case CMDHI_ESTUPDATE:            handle_estupdatemsghi(source_node, dest_node, path_nodecount, path_nodes, data_len, data); break;
      case CMDHI_CONFIRMJOIN:          handle_confirmjoinmsghi(source_node, dest_node, path_nodecount, path_nodes, data_len, data); break;
      case CMDHI_CONFIRMSOURCE:        handle_confirmsourcemsghi(source_node, dest_node, path_nodecount, path_nodes, data_len, data); break;
      case CMDHI_CONFIRMTARGETSOURCE:  handle_confirmtargetsourcemsghi(source_node, dest_node, path_nodecount, path_nodes, data_len, data); break;
      case CMDHI_TUPLE:                handle_tuplemsghi(source_node, dest_node, path_nodecount, path_nodes, data_len, data); break;
      case CMDHI_REPAIR:               handle_repairmsghi(source_node, dest_node, path_nodecount, path_nodes, data_len, data); break;
      case CMDHI_PATHCOLLAPSE:         handle_pathcollapsemsghi(source_node, dest_node, path_nodecount, path_nodes, data_len, data); break;
      case CMDHI_COORDCOST:            handle_coordcostmsghi(source_node, dest_node, path_nodecount, path_nodes, data_len, data); break;
      case CMDHI_COORDDECISION:        handle_coorddecisionmsghi(source_node, dest_node, path_nodecount, path_nodes, data_len, data); break;
      case CMDHI_COORDPAIRS:           handle_coordpairsmsghi(source_node, dest_node, path_nodecount, path_nodes, data_len, data); break;
      case CMDHI_SUMMARYSPEC:          handle_summaryspecmsghi(source_node, dest_node, path_nodecount, path_nodes, data_len, data); break;
      case CMDHI_ATTRUPDATE:           handle_attrupdatemsghi(source_node, dest_node, path_nodecount, path_nodes, data_len, data); break;
      case CMDHI_QUERY:                handle_querymsghi(source_node, dest_node, path_nodecount, path_nodes, data_len, data); break;
      case CMDHI_TREESDONE:            handle_treesdonemsghi(source_node, dest_node, path_nodecount, path_nodes, data_len, data); break;
      case CMDHI_STATS:                handle_statsmsghi(source_node, dest_node, path_nodecount, path_nodes, data_len, data); break;
      default:
        DBGERR("Invalid high-level command (%d)", data[0] & CMDHI_MASK);
        pause_sim();
        return;
    }
}

void handlefail_msghi(ADDRESS source_node, ADDRESS dest_node, uint8_t path_nodecount,
  ADDRESS *path_nodes, uint8_t data_len, uint8_t *data)   // main handler for failed payloads
{
  DBGOUT("Received failed forwarding from node %d", source_node);
  if (path_nodecount > 0)
    print_path(path_nodes, path_nodecount, "Path");

  if (data_len > 0)
  {
    if ((data[0] & CMDHI_MASK) >= CMDHI_COUNT)
    {
      DBGERR("Invalid high-level command (%d)", data[0] & CMDHI_MASK);
      pause_sim();
      return;
    } else switch (data[0] & CMDHI_MASK)
    {
      case CMDHI_CONFIRMJOIN:          handlefail_confirmjoinmsghi(source_node, dest_node, path_nodecount, path_nodes, data_len, data); break;
      case CMDHI_CONFIRMTARGETSOURCE:  handlefail_confirmtargetsourcemsghi(source_node, dest_node, path_nodecount, path_nodes, data_len, data); break;
      case CMDHI_TUPLE:                handlefail_tuplemsghi(source_node, dest_node, path_nodecount, path_nodes, data_len, data); break;
    }
  }  
}

void update_payloadbuffer()
{
  int i;
  PAYLOADBUFFER_ENTRY *p;

  for (i = 0; i < PAYLOADBUFFER_THROTTLE; i++)
  {
    if (payloadbuffer_size == 0)
      break;

    DBGOUT("Processing payload at position %d", payloadbuffer_first);
    p = &payload_buffer[payloadbuffer_first];
    (p->failed ? handlefail_msghi : handle_msghi)(p->source_node, p->dest_node, p->path_len,
      p->path, p->data_len, p->data);
    mfree(p->path);
    mfree(p->data);

    payloadbuffer_first = (payloadbuffer_first+1) % PAYLOADBUFFER_SIZE;
    payloadbuffer_size--;
  }
}

void enqueue_payload(ADDRESS source_node, ADDRESS dest_node, uint8_t path_len, ADDRESS *path,
  uint8_t data_len, uint8_t *data, uint8_t failed)
{
  PAYLOADBUFFER_ENTRY *p;

DO_STACKTEST
  
  if (payloadbuffer_size >= PAYLOADBUFFER_SIZE)
  {
    DBGERR("Cannot enqueue payload in payload buffer!");
#ifdef TOSSIM    
    log_dropcmd(1, &source_node, data, data_len, 6);
#endif
    return;
  }

  p = &payload_buffer[payloadbuffer_last];
  p->failed = failed;
  p->source_node = source_node; p->dest_node = dest_node;

  p->path_len = path_len;
  if (path_len > 0)
  {
    p->path = mcalloc(path_len, sizeof(ADDRESS));
    memcpy(p->path, path, path_len*sizeof(ADDRESS));
  } else p->path = NULL;

  p->data_len = data_len;
  if (data_len > 0)
  {
    p->data = mmalloc(data_len);
    memcpy(p->data, data, data_len);
  } else p->data = NULL;
  
  DBGOUT("Enqueued payload at position %d", payloadbuffer_last);
  payloadbuffer_last = (payloadbuffer_last+1) % PAYLOADBUFFER_SIZE;
  payloadbuffer_size++;

  if (payloadbuffer_size > maxusage_payloadbuffer)
    maxusage_payloadbuffer = payloadbuffer_size;    
}

void route_reached(ROUTE_MSG *msg)     // called when the seq. route or flood route reached the destination
{
  uint8_t path_len;
  ADDRESS path[MAX_PATHLEN];

  path_len = msg->path_len;
  memcpy(path, msg->path, path_len*sizeof(ADDRESS));

  if (path_len > 0)
  {
    path_len--;
    reverse_pathvec(path, path_len);
  }

  enqueue_payload(msg->source_node, TOS_NODE_ID, path_len, path, msg->data_len, msg->data, 0);

  total_pathlen += path_len;
#ifdef TOSSIM
  finish_route(msg);
#endif
}

void route_unreachable(ROUTE_MSG *msg)
{                  // called on a seq. route at the initiator, when no destination can be found.
  int repairpath_index;

  if (msg->is_repair)  // currently interesting only for path repairs
  {
    DBGOUT("Cannot find repair for message from %d to %d through %d", msg->source_node,
      msg->predicates[0].value, msg->failed_node);

    repairpath_index = find_repairpathindex(msg->failed_node, msg->predicates[0].value);
    if (repairpath_index < 0)
    {
      DBGERR("Cannot find repairpath entry!");
      pause_sim();
      return;
    }

    repair_path[repairpath_index].path_status = PATHSTATUS_BROKEN;
    handlerepair_forwardmsgs(ROUTE_FAIL, msg->failed_node, msg->predicates[0].value);
  }

#ifdef TOSSIM
  route_logunreachable(msg);
#endif
}

uint8_t checkallow_routeshortcut(ROUTE_MSG *msg)
{              // allow shortcutting of the pathvectors for flood routing messages, only if they are not repairs.
  return (msg->data_len == 0 || (msg->data[0] & CMDHI_MASK) != CMDHI_REPAIR);  // dont introduced failed nodes through shortcuts
}

uint32_t evalpath_default(ROUTE_MSG *msg)     // default path cost estimation: path length
{
  return 10*msg->path_len;
}

uint32_t eval_path(ROUTE_MSG *msg)     // path cost estimation
{
  return (msg->data_len > 0 && (msg->data[0] && CMDHI_MASK) == CMDHI_INIT) ? evalpath_init(msg) :
    evalpath_default(msg);
}

void handle_routeintermediate(ROUTE_MSG *msg)   // main handler for payload-dependent routing
{
DO_STACKTEST

  if (msg->data_len > 0)
  {
    if ((msg->data[0] & CMDHI_MASK) >= CMDHI_COUNT)
    {
      DBGERR("Invalid high-level command (%d)", msg->data[0] & CMDHI_MASK);
      pause_sim();
      return;
    } else switch (msg->data[0] & CMDHI_MASK)
    {
      case CMDHI_INIT:   handlerouteinter_init(msg); break;
      case CMDHI_REPAIR: handlerouteinter_repair(msg); break;
    }
  }  
}

void print_predicates(uint8_t predicate_count, ROUTING_PREDICATE *predicates)
{
  int i, len;
  char s[256] = "";

  for (i = 0; i < predicate_count; i++)
  {
    len = strlen(s);
#ifdef TOSSIM    
    sprintf(&s[len], "%s=%d(%d), ", attr_name[summary_desc[predicates[i].summary_index].attrdesc_index],
      predicates[i].value, predicates[i].value_ext);
#else
    sprintf(&s[len], "%d=%d(%d), ", summary_desc[predicates[i].summary_index].attrdesc_index,
      predicates[i].value, predicates[i].value_ext);
#endif      
  }

  DBGOUT("Predicates: %s", s);
}

uint8_t init_route(uint8_t predicate_count, ROUTING_PREDICATE *predicates, uint8_t method,
  uint8_t data_len, uint8_t *data)   
{                                  // initializes a new routing request with given parameters and payload supplied
  ROUTE_MSG msg;

DO_STACKTEST

  DBGOUT("Initiating %s route", (method == 0) ? "seq." : "flood");
  print_predicates(predicate_count, predicates);
  if (data_len > MAX_ROUTEDATASIZE)
  {
    DBGERR("Data size too large: %d", data_len);
    pause_sim();
    return 1;
  }

  msg.cmd = (method == 0) ? CMD_SEQROUTE : CMD_FLOODROUTE;
  msg.route_id = route_seq;
  route_seq = (route_seq+1) % 127;

  msg.predicate_count = predicate_count;
  memcpy(msg.predicates, predicates, predicate_count*sizeof(ROUTING_PREDICATE));

  msg.tree_index = 0;
  msg.source_node = TOS_NODE_ID;
  msg.path_len = 1;
  msg.path[0] = TOS_NODE_ID; //initial top node is current node
  msg.top_index = 0;
  msg.shortcut_index = -1;
  msg.is_repair = 0;

  msg.data_len = data_len;
  memcpy(msg.data, data, data_len);

#ifdef TOSSIM
  route_request(&msg);
  if (config.disable_msglog == 0)
    log_sendcmdhi(data, data_len);
#endif

  if (method == 0)
    seq_route(TOS_NODE_ID, &msg);
  else flood_route(TOS_NODE_ID, &msg);

  return 0;
}

uint8_t init_repairroute(ADDRESS dest_node, ADDRESS failed_node, uint8_t path_len, ADDRESS *path,
  uint8_t data_len, uint8_t *data)  // initializes a new RepairRoute request, with given failed path fragment, and payload
{
  ROUTE_MSG msg;

DO_STACKTEST

  DBGOUT("Initiating repair route from node %d to node %d", TOS_NODE_ID, dest_node);
  if (data_len > MAX_ROUTEDATASIZE)
  {
    DBGERR("Data size too large: %d", data_len);
    pause_sim();
    return 1;
  }

  msg.cmd = CMD_SEQROUTE;
  msg.route_id = route_seq;
  route_seq = (route_seq+1) % 127;

  msg.predicate_count = 1;
  msg.predicates[0].summary_index = find_repairsummary();
  msg.predicates[0].value = dest_node;
  msg.predicates[0].value_ext = 0;

  msg.tree_index = 0;
  msg.source_node = TOS_NODE_ID;
  msg.path_len = 1;
  msg.path[0] = TOS_NODE_ID; //initial top node is current node
  msg.top_index = 0;
  msg.shortcut_index = -1;

  msg.is_repair = 1;
  msg.repair_hoplimit = MAX_REPAIRPATHLEN;
  msg.failed_node = failed_node;
  msg.fail_count = 0;
  if (path_len > MAX_REPAIRPATHLEN)
    path_len = MAX_REPAIRPATHLEN;
  msg.repair_pathlen = path_len;
  memcpy(msg.repair_path, path, path_len*sizeof(ADDRESS));

  msg.data_len = data_len;
  memcpy(msg.data, data, data_len);

#ifdef TOSSIM
  route_request(&msg);
  if (config.disable_msglog == 0)
    log_sendcmdhi(data, data_len);
#endif

  seq_route(TOS_NODE_ID, &msg);

  return 0;
}

uint8_t init_baseflood(uint8_t predicate_count, ROUTING_PREDICATE *predicates, uint8_t data_len,
  uint8_t *data)
{
  BASEFLOOD_MSG msg;

DO_STACKTEST

  DBGOUT("Initiating base flood from node %d", TOS_NODE_ID);
  print_predicates(predicate_count, predicates);
  if (data_len > MAX_BASEFLOODDATASIZE)
  {
    DBGERR("Data size too large: %d", data_len);
    pause_sim();
    return 1;
  }

  msg.cmd = CMD_BASEFLOOD;
  msg.predicate_count = predicate_count;
  memcpy(msg.predicates, predicates, predicate_count*sizeof(ROUTING_PREDICATE));
  msg.source_node = TOS_NODE_ID;

  msg.data_len = data_len;
  memcpy(msg.data, data, data_len);

#ifdef TOSSIM
  if (config.disable_msglog == 0)
    log_sendcmdhi(data, data_len);
#endif

  base_flood(TOS_NODE_ID, &msg);

  return 0;
}

void send_seqroutemsg(uint8_t direction, ROUTE_MSG *p)   // marshal and send out a seq. route message
{
  ADDRESS dest;
  uint8_t *buffer;
  int size;

DO_STACKTEST

  if (direction == ROUTEMSG_FORWARD)
  {
    p->cmd = CMD_SEQROUTE;
    dest = p->path[p->path_len-1];                  // obtain next hop from path
  } else {
    if (p->path_len < 2)
    {
      DBGERR("Cannot route to destination!");
      route_unreachable(p);
      return;
    }

    p->cmd = CMD_SEQROUTEUNR;
    dest = p->path[p->path_len-2];                  // obtain prev hop from path
  }

  DBGOUT("Forwarding routing message to node %d", dest);
  buffer = (uint8_t *) mmalloc(marshalest_routemsg(p));
  size = marshal_routemsg(p, buffer);
  send_genericmsg(buffer, size, MSG_CONFIRM, 1, alloc_addr(dest), -1, PEER_DEFAULTPOWER, 1);
  mfree(buffer);

  msgsent_route++;
  bytesent_route += size;
}

void send_floodroutemsg(uint8_t node_count, ADDRESS *nodes, ROUTE_MSG *p)  // send out a forwarding msgs
{
  uint8_t *buffer;
  int size;
  
DO_STACKTEST

  p->cmd = CMD_FLOODROUTE;
  buffer = (uint8_t *) mmalloc(marshalest_routemsg(p));
  size = marshal_routemsg(p, buffer);
  send_genericmsg(buffer, size, MSG_CONFIRM, node_count, nodes, -1, PEER_DEFAULTPOWER, 1);
  mfree(buffer);

  msgsent_route++;
  bytesent_route += size;
}

int findfree_routeentry()    // return a free entry (if any) if the route buffer
{
  int i;

  for (i = 0; i < ROUTEBUFFER_SIZE; i++)
    if (route_buffer[i].used == 0)
      return i;

  return -1;
}

int find_routeentry(ROUTE_MSG *msg)     // search the route buffer for a given request
{
  int i;

  for (i = 0; i < ROUTEBUFFER_SIZE; i++)
    if (route_buffer[i].used && route_buffer[i].route_id == msg->route_id &&
      route_buffer[i].source_node == msg->source_node)
      return i;

  return -1;
}

int point_dist(ATTRIBUTE p1, ATTRIBUTE p2)   // euclidean distance between two points
{
  int px1 = p1 & 0xff, py1 = p1 >> 8, px2 = p2 & 0xff, py2 = p2 >> 8;
  return sqrt(SQR(px1-px2)+SQR(py1-py2));
}

uint8_t check_doneroute(uint8_t predicate_count, ROUTING_PREDICATE *predicates)
{                // routing is done if value of our attribute is equal or in range of the target attribute
  int i;

  for (i = 0; i < predicate_count; i++)
    if (predicates[i].value != attr_desc[summary_desc[predicates[i].summary_index].attrdesc_index].value &&
      (summary_desc[predicates[i].summary_index].type != SUMMARY_RTREE ||
      point_dist(predicates[i].value,
        attr_desc[summary_desc[predicates[i].summary_index].attrdesc_index].value) >=
        predicates[i].value_ext))
      return 0;

  return 1;
}

int check_forwardpeer(uint8_t peer_index, uint8_t tree, uint8_t predicate_count,
  ROUTING_PREDICATE *predicates)
{                            // returns 1 if we should be forwarding the request to neighbor "peer_index" on tree "tree"
  int i;

  if (tree >= config.trees_enabled || peer_index >= MAX_PEERLINKS)
    return 0;

  for (i = 0; i < predicate_count; i++)
    if (summary_test(peer_link[peer_index].tree.trees[tree].summary[predicates[i].summary_index],
      predicates[i].value, predicates[i].value_ext) == 0)
      return 0;

  return 1;
}

int check_canforward(ADDRESS source_id, uint8_t predicate_count, ROUTING_PREDICATE *predicates,
  uint8_t trees_enabled)
{                            // returns 1 if we should be forwarding the request to any child
  int i, j;
  uint8_t can_forward;

  for (i = 0; i < trees_enabled; i++)
    if (trees[i].parent == source_id)
    {
      can_forward = 1;
      for (j = 0; j < predicate_count; j++)
        if (summary_test(trees[i].summary[predicates[j].summary_index], predicates[j].value,
          predicates[j].value_ext) == 0)
        {
          can_forward = 0;
          break;
        }

      if (can_forward)
        return 1;
    }

  return 0;
}

uint8_t is_root()     // returns 1 if current node is a root in any tree
{
  int i;

  for (i = 0; i < config.trees_enabled; i++)
    if (trees[i].hops == 0)
      return 1;

  return 0;
}

uint8_t sourceroute_contains(ADDRESS source_id, ROUTE_MSG *msg, ADDRESS node)
{                         // returns 1 if the source route of the request contains "node"
  int i;

  if (msg->source_node == node || (source_id == node && msg->cmd == CMD_SEQROUTE))
    return 1;

#if ATTR_BITS >= ATTRDOMAIN_BITS
  for (i = 0; i < msg->path_len-(msg->cmd == CMD_SEQROUTEUNR); i++)
    if (msg->path[i] == node)  // subtracting (msg->cmd == CMD_SEQROUTEUNR): last entry - where it came from if unreachble,
      return 1;                // this entry could be explored again
#endif

  return 0;
}

int neighborshortcut_index(ATTRIBUTE id)   // return index of neighbor to shortcut to, if any
{
  int i, t;

  if (id == BASESTATION_ADDR)  // if looking for basestation, shortcut to parent in tree 0
    for (i = 0; i < MAX_PEERLINKS; i++)
      if (peer_link[i].used && peer_link[i].id == trees[0].parent && peerlink_statusok(i))
        return i;

  for (i = 0; i < MAX_PEERLINKS; i++)
    if (peer_link[i].used && peer_link[i].id == id && peerlink_statusok(i))
    {
      DBGOUT("Found among immediate neighbors. Forwarding to %d", peer_link[i].id);
      return i;
    }

  for (i = 0; i < MAX_PEERLINKS; i++)
    if (peer_link[i].used && peerlink_statusok(i))
    {
      for (t = 0; t < config.trees_enabled; t++)
        if (peer_link[i].tree.trees[t].parent == id)
        {
          DBGOUT("Found among parents of immediate neighbors. Forwarding to %d", peer_link[i].id);
          return i;
        }

      if (find_extneighborhoodentry(peer_link[i].id, id) >= 0)
      {
        DBGOUT("Found in extended neighborhood. Forwarding to %d", peer_link[i].id);
        return i;
      }
    }

  return -1;
}

void shortcut_routemsg(ROUTE_MSG *msg)     // find shortcuts in source route
{
  int i, j, t;

  print_path(msg->path, msg->path_len, "Old path");

  for (i = 0; i < msg->path_len-2; i++)
  {
    for (j = 0; j < MAX_PEERLINKS; j++)   // one hop shortcuts
      if (peer_link[j].used && peerlink_statusok(j) && peer_link[j].id == msg->path[i])
      {
        msg->shortcut_index = j;
        if (msg->top_index == msg->path_len-1)
          msg->top_index = i+1;
        else if (msg->top_index > i)
          msg->top_index = i;

        msg->path_len = i+2;
        msg->path[i+1] = TOS_NODE_ID;

        DBGOUT("Shortcut found from %d", msg->path[i]);
        DBGOUT("Shortcut index %d, top index: %d", msg->shortcut_index, msg->top_index);
        print_path(msg->path, msg->path_len, "New path");

        return;
      }

    if (i < msg->path_len-3)
      for (j = 0; j < MAX_PEERLINKS; j++)   // two hop shortcuts
        if (peer_link[j].used && peerlink_statusok(j))
        {
          for (t = 0; t < config.trees_enabled; t++)
            if (peer_link[j].tree.trees[t].parent == msg->path[i])
            {
              msg->shortcut_index = j;
              if (msg->top_index == msg->path_len-1)
                msg->top_index = i+2;
              else if (msg->top_index > i)
                msg->top_index = i;

              msg->path_len = i+3;
              msg->path[i+1] = peer_link[j].id;
              msg->path[i+2] = TOS_NODE_ID;

              DBGOUT("Double shortcut found from %d", msg->path[i]);
              DBGOUT("Neighbor %d's parent %d in tree %d", peer_link[j].id, peer_link[j].tree.trees[t].parent, t);
              DBGOUT("Shortcut index %d, top index: %d", msg->shortcut_index, msg->top_index);
              print_path(msg->path, msg->path_len, "New path");

              return;
            }

          if (id_to_peerlink(msg->path[i]) < 0 && find_extneighborhoodentry(peer_link[j].id, msg->path[i]) >= 0)
          {
            msg->shortcut_index = j;
            if (msg->top_index == msg->path_len-1)
              msg->top_index = i+2;
            else if (msg->top_index > i)
              msg->top_index = i;

            msg->path_len = i+3;
            msg->path[i+1] = peer_link[j].id;
            msg->path[i+2] = TOS_NODE_ID;

            DBGOUT("Double shortcut found from %d", msg->path[i]);
            DBGOUT("Neighbor %d link with %d", peer_link[j].id, msg->path[i]);
            DBGOUT("Shortcut index %d, top index: %d", msg->shortcut_index, msg->top_index);
            print_path(msg->path, msg->path_len, "New path");

            return;
          }
        }
  }
}

void handle_routebeforesend(ROUTE_MSG *msg)    // perform intermediate and payload-dependent operations
{
  if (checkallow_routeshortcut(msg))
    shortcut_routemsg(msg);

  if (msg->data_len > 0)
    handle_routeintermediate(msg);
}

typedef struct {
  int index, v;
} PACKING_MACRO PEERSEQ_ENTRY;

void prepare_peerseq(ADDRESS source_id, ROUTE_MSG *msg, PEERSEQ_ENTRY *peer_seq)
{                                     // for seq route, prepare exploration list of neighbors, most promising ones first
  int i, j, k, min, best_tree, t;
  PEERSEQ_ENTRY temp;
#ifdef TOSSIM
  char s[256];
#endif

DO_STACKTEST

  for (i = 0; i < MAX_PEERLINKS; i++)   // generate sequence for children
    if (peer_link[i].used == 0 || sourceroute_contains(source_id, msg, peer_link[i].id) ||
      (msg->is_repair && peer_link[i].id == msg->failed_node && msg->fail_count >= MAX_FAILRETRY))
    {
      peer_seq[i].index = -1;       // ignore neighbors which are not children, create loops, failed, etc
      peer_seq[i].v = INT_MAX;
    } else {
      best_tree = -1;               // for the other ones, find the max depth of their best subtree
      for (j = 0; j < config.trees_enabled; j++)
        if (peer_link[i].tree.trees[j].parent == TOS_NODE_ID &&
          check_forwardpeer(i, j, msg->predicate_count, msg->predicates) && (best_tree == -1 ||
          (peer_link[i].tree.trees[j].deepest_hops-trees[j].hops) <
          (peer_link[i].tree.trees[best_tree].deepest_hops-trees[best_tree].hops))) 
          best_tree = j;

      if (best_tree == -1)
      {
        peer_seq[i].index = -1;
        peer_seq[i].v = INT_MAX;
      } else {
        peer_seq[i].index = i;
        peer_seq[i].v = peer_link[i].tree.trees[best_tree].deepest_hops-trees[best_tree].hops;
      }
    }

  for (i = 0; i < MAX_PEERLINKS-1; i++)   // sort children by max depth
  {
    min = i;
    for (j = i+1; j < MAX_PEERLINKS; j++)
      if (peer_seq[min].index == -1 || (peer_seq[j].index != -1 && peer_seq[j].v < peer_seq[min].v))
        min = j;

    temp = peer_seq[min];
    peer_seq[min] = peer_seq[i];
    peer_seq[i] = temp;
  }

  if (msg->path_len-msg->top_index-1 <= (msg->cmd == CMD_SEQROUTEUNR) && is_root() == 0)
    for (i = 0; i < MAX_PEERLINKS; i++)   // if top node and not root in any tree, add parents
      if (peer_seq[i].index == -1)
      {
        if (msg->source_node == TOS_NODE_ID)    // if we are the originator of the request
        {
          k = 0;
          for (j = 0; j < config.trees_enabled; j++)    // consider all tress
          {
            t = (TOS_NODE_ID+msg->route_id+j) % config.trees_enabled;  // create a random tree permutation
            if (msg->is_repair == 0 || trees[t].parent != msg->failed_node || msg->fail_count < MAX_FAILRETRY)
            {                                                      // if doing repair, send only to those which are not failed
              peer_seq[i+k].index = t;
              peer_seq[i+k].v = -1;
              k++;
            }
          }
        } else {
          peer_seq[i].index = msg->tree_index;   // if we are not the initiator, forward on the same tree used to forward to us.
          peer_seq[i].v = -1;
        }

        break;
      }

#ifdef TOSSIM                                    // prints the sequence
  memset(s, 0, sizeof(s));
  for (i = 0; i < MAX_PEERLINKS+TREE_COUNT; i++)
  {
    if (peer_seq[i].index < 0)
      break;

    if (peer_seq[i].v != -1)
      sprintf(&s[strlen(s)], "(Node %d, max hops %d) ", peer_link[peer_seq[i].index].id,
        peer_seq[i].v);
    else sprintf(&s[strlen(s)], "(Node %d, parent in %d) ", trees[peer_seq[i].index].parent,
      peer_seq[i].index);
  }

  DBGOUT("Sequence: %s", s);
#endif
}

uint8_t check_pathtoolong(ROUTE_MSG *msg)           // check if we are not exceding path limits for normal route or RepairRoute
{
  return ((msg->is_repair && msg->path_len >= msg->repair_hoplimit) ||
    (msg->is_repair == 0 && msg->path_len >= MAX_PATHLEN));
}

void traverse_peerseq(ROUTE_MSG *msg, PEERSEQ_ENTRY *peer_seq)
{                                     // for a given request and exploration sequence, find the next neighbor to explroe
  int i;

  if (msg->cmd == CMD_SEQROUTE)   // we are not returning from anywhere, so we must be starting from the beginning of the sequence
  {
    DBGOUT("New request.");
    if (check_pathtoolong(msg) || peer_seq[0].index == -1)
    {
      DBGERR("Sequence is empty or path too long!");
      send_seqroutemsg(ROUTEMSG_RETURN, msg);
    } else {
      if (peer_seq[0].v == -1)                 // forward to first node in list
      {
        DBGOUT("Forwarding to parent %d in tree %d", trees[peer_seq[0].index].parent,
          peer_seq[0].index);
        msg->tree_index = peer_seq[0].index;    // add next hop to path and update top index
        msg->path[msg->path_len++] = trees[peer_seq[0].index].parent;
        msg->top_index++;
      } else {
        DBGOUT("Forwarding to child %d", peer_link[peer_seq[0].index].id);
        msg->path[msg->path_len++] = peer_link[peer_seq[0].index].id;  // add next hop to path
      }

      send_seqroutemsg(ROUTEMSG_FORWARD, msg);
    }
  } else {
    DBGOUT("Existing request.");  // we're returning from previous exploration, must find our position in the sequence

    for (i = 0; i < MAX_PEERLINKS+TREE_COUNT; i++)
      if (peer_seq[i].index >= 0)
      {                                                 // our parent said they cant reach the destination
        if (msg->path_len-msg->top_index == 1 && peer_seq[i].v == -1 &&
          peer_seq[i].index == msg->tree_index)
        {
          DBGOUT("Received unreachable message from parent %d in tree %d",
            trees[peer_seq[i].index].parent, peer_seq[i].index);
          msg->path_len--;
          msg->top_index--;                            // our child said they cant reach the destination
        } else if (msg->path_len-msg->top_index > 1 && peer_seq[i].v > 0 &&
          peer_link[peer_seq[i].index].id == msg->path[msg->path_len-1])
        {
          DBGOUT("Received unreachable message from child %d", peer_link[peer_seq[i].index].id);
          msg->path_len--;
        } else
          continue;

        if (check_pathtoolong(msg) == 0 && peer_seq[i+1].index >= 0)
        {
          if (peer_seq[i+1].v == -1)
          {
            DBGOUT("Forwarding to parent %d in tree %d", trees[peer_seq[i+1].index].parent,
              peer_seq[i+1].index);
            msg->tree_index = peer_seq[i+1].index;
            msg->path[msg->path_len++] = trees[peer_seq[i+1].index].parent;
            msg->top_index++;
          } else {
            DBGOUT("Forwarding to child %d", peer_link[peer_seq[i+1].index].id);
            msg->path[msg->path_len++] = peer_link[peer_seq[i+1].index].id;
          }

          send_seqroutemsg(ROUTEMSG_FORWARD, msg);
        } else {
          DBGERR("Traversed the entire sequnce or path too long!");
          send_seqroutemsg(ROUTEMSG_RETURN, msg);
        }

        return;
      } else {
        DBGERR("Peer sequence at index %d is negative", i);
        pause_sim();
        break;
      }

    DBGERR("Cannot find unreachable message in sequnce!");
    pause_sim();
    return;
  }
}

uint8_t handle_repair(ROUTE_MSG *msg)      // check if RepairRoute finished without actually getting to the destination
{                                          // if we arrived on the portion of the path after the failure
  int i, peer_index;

  for (i = 1; i < msg->repair_pathlen; i++)
    if (msg->repair_path[i] == TOS_NODE_ID)
    {
      DBGOUT("Current node appears to be on the source route at position %d", i);
      msg->shortcut_index = i;
      handle_routebeforesend(msg);
      route_reached(msg);
      return 1;
    }

  for (i = msg->repair_pathlen-1; i > 0; i--)  // if thats not the case, check for shortcuts to one of the nodes on the repair path
  {
    peer_index = id_to_peerlink(msg->repair_path[i]);  // do not perform double-shortcuts here
    if (peer_index >= 0 && peerlink_statusok(peer_index))
    {
      DBGOUT("Found among neighbors in repair path");
      handle_routebeforesend(msg);
      msg->path[msg->path_len++] = peer_link[peer_index].id;
      send_seqroutemsg(ROUTEMSG_FORWARD, msg);
      return 1;
    }
  }

  return 0;
}

int allocate_routeentry(ROUTE_MSG *msg)  // allocate routing state, in the route buffer
{
  int route_entry;

  route_entry = find_routeentry(msg);
  if (route_entry < 0)
  {
    route_entry = findfree_routeentry();
    if (route_entry < 0)
    {
      route_entry = rand() % ROUTEBUFFER_SIZE;
      DBGERR("No free entries. Randomly evicting entry %d", route_entry);
    } else routebuffer_size++;

    route_buffer[route_entry].used = 1;
    route_buffer[route_entry].route_id = msg->route_id;
    route_buffer[route_entry].source_node = msg->source_node;
    if (msg->cmd == CMD_FLOODROUTE)
    {
      route_buffer[route_entry].visited_type = 0;          // for flooding routing, visited type is ignored
      route_buffer[route_entry].min_cost = 0xff;
    } else {
      route_buffer[route_entry].visited_type = (msg->path_len-msg->top_index > 1);    // for seq. routing, visited type
      route_buffer[route_entry].min_cost = eval_path(msg);                            // and hops considered
    }
  }

  return route_entry;
}

void seq_route(ADDRESS source_id, ROUTE_MSG *msg)    // processes a sequential routing message, received from neighbor source_id
{                                                    // source_id might be TOS_NODE_ID if we're just starting the request
  int visited_type, route_entry, peer_index;
  PEERSEQ_ENTRY peer_seq[MAX_PEERLINKS+TREE_COUNT+1];

DO_STACKTEST
                                           
  DBGOUT("Seq. route received from node %d", source_id);  // display debugging information
  DBGOUT("Route id: %d, Source: %d, Top index: %d, Data len: %d, Unreachable: %d", msg->route_id,
    msg->source_node, msg->top_index, msg->data_len, msg->cmd == CMD_SEQROUTEUNR);
  print_predicates(msg->predicate_count, msg->predicates);
  if (msg->path_len > 0)
    print_path(msg->path, msg->path_len, "Path");
  if (msg->is_repair && msg->repair_pathlen > 0)
    print_path(msg->repair_path, msg->repair_pathlen, "Repair path");
  if (msg->data_len > 0)
    print_list(msg->data, msg->data_len, "Data");

  if (check_doneroute(msg->predicate_count, msg->predicates))  // check for termination
  {
    if (msg->source_node != TOS_NODE_ID)     // we're really done if we are not the initiator of the request
    {
      DBGOUT("Done routing");
      if (msg->is_repair && msg->repair_pathlen > 0)
        msg->shortcut_index = msg->repair_pathlen-1;
      handle_routebeforesend(msg);
      route_reached(msg);
      return;
    }

    if (IS_IDROUTE(msg)) // IDs are assumed to be unique, do nothing if initiator looks for own ID
      return;         
  }

  route_entry = find_routeentry(msg);
  visited_type = (route_entry == -1) ? -1 : route_buffer[route_entry].visited_type;
                                          // visited type: -1 - first time, 0 - visited as ascending, 1 - as descending
  DBGOUT("Route entry %d, visited type %d", route_entry, visited_type);

  if (msg->cmd == CMD_SEQROUTE && (visited_type == 0 ||     // allow repeated visit only if previously visited as descending,
    (visited_type == 1 && msg->path_len-msg->top_index > 1)))     // and now as ascending
  {
    DBGOUT("Already visited!");
    send_seqroutemsg(ROUTEMSG_RETURN, msg); // node already visited as ascending or descending
    return;
  }

  if (msg->cmd == CMD_SEQROUTEUNR && visited_type < 0) // previous shortcut has failed as unreachable
  {
    DBGOUT("Received unreachable request with no state. Resetting to forward request.");
    msg->cmd = CMD_SEQROUTE;
    if (msg->path_len > 1)
      msg->path_len--;
  }

  if (msg->is_repair)               // if we are doing a repair...
  {
    if (handle_repair(msg))             // ...and we find the target on the repair path, then we're done
      return;                           
  } else if (IS_IDROUTE(msg))           // if not doing repair, check immediate neighbors if looking for id
  {                                     
    peer_index = neighborshortcut_index(msg->predicates[0].value);  
    if (peer_index >= 0)
    {
      DBGOUT("Found among immediate neighbors.");
      handle_routebeforesend(msg);
      msg->path[msg->path_len++] = peer_link[peer_index].id;
      send_seqroutemsg(ROUTEMSG_FORWARD, msg);
      return;
    }
  }

  if (msg->cmd == CMD_SEQROUTE)           // if we are exploring
  {                                       // check to see if our parent's summary had a false positive
    if (msg->path_len-msg->top_index > 1 &&
      check_canforward(source_id, msg->predicate_count, msg->predicates, config.trees_enabled) == 0)
    {                                     // if yes, return to sender, without leaving state
      DBGOUT("Cannot forward message. Value not in summary");
      send_seqroutemsg(ROUTEMSG_RETURN, msg);
      return;
    }

    if (visited_type < 0)                  // if this is the first time we visit this node
      route_entry = allocate_routeentry(msg);  // allocate an entry in the route buffer
    else {
      route_buffer[route_entry].visited_type = 0; // now visited as parent
      route_buffer[route_entry].min_cost = eval_path(msg);
    }
  }

  prepare_peerseq(source_id, msg, peer_seq);            // prepare the exploration sequence of neighbors
  traverse_peerseq(msg, peer_seq);                      // and then explore the next neighbor on the sequence
}

uint8_t is_parent(ADDRESS node)       // returns 1 if "node" is our parent in any tree
{
  int t;

  for (t = 0; t < config.trees_enabled; t++)
    if (trees[t].parent == node)
      return 1;

  return 0;
}

uint8_t is_child(ADDRESS source_id, ROUTE_MSG *msg, uint8_t update_treeindex)
{                      // returns 1 if source_id is our child in the correct tree, as specified by "msg"
  int i, peer_index;   // if update_treeindex is 1, then tree_index of msg is updated to the tree in which we are parent of the initiator
                       // WARNING: this can be called with header-unmarshalled route_msg from checkaccept; dont use payload
  if (source_id == TOS_NODE_ID)
    return 1;          // pretend we are our own child, if we are the initiator
  else {
    peer_index = id_to_peerlink(source_id);
    if (peer_index < 0)
    {
      DBGERR("Node %d not in peer list", source_id);
      return 0;
    }

    if (source_id == msg->source_node)
    {
      for (i = 0; i < config.trees_enabled; i++)
        if (peer_link[peer_index].tree.trees[i].parent == TOS_NODE_ID) // if we are the initiator's parent...
        {
          if (update_treeindex)                          // ...and update_treeindex is set
            msg->tree_index = i;                         // update the tree_index field
          return 1;                                      // this is needed to allow the initiator to use only one message
        }                                                // to send to all its parents. Each parent adjusts its tree_index

      return 0;
    } else
      return (peer_link[peer_index].tree.trees[msg->tree_index].parent == TOS_NODE_ID);   // if we're not receiving from the initiator
  }                                                                              // check if we are the correct parent
}

void generate_destlist(ADDRESS source_id, ROUTE_MSG *msg, uint8_t add_parent, uint8_t add_children,
 uint8_t *node_count, ADDRESS *nodes)
{                    // generate a list of neighbors to forward flood route
  int i, t;          // if add_parent == 1, consider parent (parents if I am initiator)
#ifdef TOSSIM        // if add_children == 1, consider children in all trees
  char s[256];
#endif  

  *node_count = 0;
  for (i = 0; i < MAX_PEERLINKS; i++)
    if (peer_link[i].used && sourceroute_contains(source_id, msg, peer_link[i].id) == 0 &&
      peer_link[i].id != source_id)
      for (t = 0; t < config.trees_enabled; t++)
        if ((add_children && peer_link[i].tree.trees[t].parent == TOS_NODE_ID &&
          check_forwardpeer(i, t, msg->predicate_count, msg->predicates)) ||
          (add_parent && trees[t].parent == peer_link[i].id &&
          (msg->source_node == TOS_NODE_ID || t == msg->tree_index)))
        {
          nodes[(*node_count)++] = peer_link[i].id;
          break;
        }

#ifdef TOSSIM
  memset(s, 0, sizeof(s));
  for (i = 0; i < *node_count; i++)
    sprintf(&s[strlen(s)], "(Node %d) ", nodes[i]);
  DBGOUT("Destinations: %s", s);
#endif
}

uint8_t checkaccept_floodroute(ADDRESS source_id, ROUTE_MSG *msg)
{                        // return 1 if this flood routing should be considered by us
  if (check_doneroute(msg->predicate_count, msg->predicates) ||
    (IS_IDROUTE(msg) && neighborshortcut_index(msg->predicates[0].value) >= 0))
    return 1;           // it is considered by us if we are the destination, or we have a shortcut to the destination

  if (is_child(source_id, msg, 0) == 0 || msg->path_len-msg->top_index > 1)
  {                                  // if the sender is not our child, or the packet is descending...
    if (is_parent(source_id) == 0)   // ... and the sender is not our parent either, then reject
      return 0;
    if (check_canforward(source_id, msg->predicate_count, msg->predicates, config.trees_enabled) == 0)
      return 0;                       // if we are not even on the right tree, then also reject
  }

  return 1;
}

void flood_route(ADDRESS source_id, ROUTE_MSG *msg)   // handle flood routing request from a source_node
{                                                     // source_node could be TOS_NODE_ID - then we are the initiator
  int route_entry, peer_index;
  uint8_t source_is_child, source_is_parent, packet_ascending, better_cost;
  ADDRESS nodes[MAX_PEERLINKS], *n;
  uint8_t node_count, path_cost;

DO_STACKTEST

  DBGOUT("Flood routing message received from node %d", source_id);  // display debugging info
  DBGOUT("Route id: %d, Source: %d, Top index: %d, Data len: %d", msg->route_id, msg->source_node,
    msg->top_index, msg->data_len);
  print_predicates(msg->predicate_count, msg->predicates);
  if (msg->path_len > 0)
    print_path(msg->path, msg->path_len, "Path");
  if (msg->data_len > 0)
    print_list(msg->data, msg->data_len, "Data");

  if (msg->path_len >= MAX_PATHLEN)
  {
    DBGERR("Path too long!");
    return;
  }

  if (check_doneroute(msg->predicate_count, msg->predicates))
  {
    DBGOUT("Done routing");
    if (msg->source_node != TOS_NODE_ID)    // we're really done if we're not the originator node
    {
      msg->path[msg->path_len++] = TOS_NODE_ID;
      handle_routebeforesend(msg);
      route_reached(msg);
      msg->path_len--;
    }

    if (IS_IDROUTE(msg)) // quit if that was a search on ID (no duplicates assumed)
      return;
  }

  if (IS_IDROUTE(msg))  // check immediate neighbors if looking for id
  {
    peer_index = neighborshortcut_index(msg->predicates[0].value);
    if (peer_index >= 0)
    {
      route_entry = allocate_routeentry(msg);
      if (route_entry < 0)
        return;

      path_cost = eval_path(msg);
      if (path_cost < route_buffer[route_entry].min_cost) // shortcut only if improvement found
      {
        route_buffer[route_entry].min_cost = path_cost;
        if (msg->source_node != TOS_NODE_ID)
        {
          msg->path[msg->path_len++] = TOS_NODE_ID;
          handle_routebeforesend(msg);
        }
        send_floodroutemsg(1, alloc_addr(peer_link[peer_index].id), msg);
      }

      return;
    }
  }

  source_is_child = is_child(source_id, msg, 1); //1: adjust tree_index if msg received from source_node
  source_is_parent = is_parent(source_id);
  DBGOUT("Source is child: %d, is parent: %d", source_is_child, source_is_parent);
  if (source_is_child == 0 && source_is_parent == 0)
  {
    DBGOUT("Rejected because source is neither parent nor child");
    return;
  }
  if (source_is_parent == 0 && msg->path_len-msg->top_index > 1)
  {
    DBGOUT("Rejected because parent received descending packet");
    return;
  }

  packet_ascending = (msg->path_len-msg->top_index == 1) && source_is_child;
  DBGOUT("Packet ascending: %d", packet_ascending);
  if (packet_ascending == 0 &&
    check_canforward(source_id, msg->predicate_count, msg->predicates, config.trees_enabled) == 0)
  {                                         // check if we're going down the right tree
    DBGOUT("Cannot forward message. Value not in summary");
    return;
  }

  route_entry = allocate_routeentry(msg);   // allocate routing state
  if (route_entry < 0)
    return;

  path_cost = eval_path(msg);
  better_cost = (path_cost < route_buffer[route_entry].min_cost);  // estimate path cost

  node_count = 0;                                                  // add children only of path cost has improved
  generate_destlist(source_id, msg, packet_ascending, better_cost, &node_count, nodes);
  if (better_cost)
    route_buffer[route_entry].min_cost = path_cost;

  if (msg->source_node != TOS_NODE_ID)                             // update source route
  {
    msg->path[msg->path_len++] = TOS_NODE_ID;
    if (packet_ascending)
      msg->top_index++;
  }

  if (node_count > 0)
  {
    handle_routebeforesend(msg);

    n = (ADDRESS *) mcalloc(node_count, sizeof(ADDRESS));
    memcpy(n, nodes, node_count*sizeof(ADDRESS));
    send_floodroutemsg(node_count, n, msg);
  }
}

uint8_t checkaccept_baseflood(ADDRESS source_id, BASEFLOOD_MSG *msg)
{
  return (source_id == trees[0].parent);
}

void base_flood(ADDRESS source_id, BASEFLOOD_MSG *msg)
{
  ADDRESS nodes[MAX_PEERLINKS], *n;
  uint8_t node_count, buffer[sizeof(BASEFLOOD_MSG)];
  int i, size;

  if (check_congestion())
  {
    DBGOUT("Dropping base routing message because of congestion");
#ifdef TOSSIM    
    log_dropcmd(1, &msg->source_node, msg->data, msg->data_len, 2);
#endif    
    return;
  }

  DBGOUT("Base flood message received from %d", source_id);
  DBGOUT("Source: %d, Data len: %d", msg->source_node, msg->data_len);
  print_predicates(msg->predicate_count, msg->predicates);
  if (msg->data_len > 0)
    print_list(msg->data, msg->data_len, "Data");
  
  if (check_doneroute(msg->predicate_count, msg->predicates))
  {
    DBGOUT("Done routing");
    enqueue_payload(msg->source_node, TOS_NODE_ID, 0, NULL, msg->data_len, msg->data, 0);
  }
  
  if (tree_phase == TREE_COUNT && msg->data_len > 0 && (msg->data[0] & CMDHI_MASK) == CMDHI_ATTRUPDATE && 
    filterbaseflood_attrupdate(&msg->data_len, msg->data))  
  {                                          // do not filter out if no summaries constructed yet
    DBGOUT("Payload filtered");  
    return;
  }

  node_count = 0;
  for (i = 0; i < MAX_PEERLINKS; i++)
    if (peer_link[i].used && peer_link[i].tree.trees[0].parent == TOS_NODE_ID &&
      check_forwardpeer(i, 0, msg->predicate_count, msg->predicates))
      nodes[node_count++] = peer_link[i].id;
      
  if (node_count > 0)
  {
    print_path(nodes, node_count, "Sending to children");

    n = (ADDRESS *) mcalloc(node_count, sizeof(ADDRESS));
    memcpy(n, nodes, node_count*sizeof(ADDRESS));
    size = marshal_basefloodmsg(msg, buffer);
    send_genericmsg(buffer, size, MSG_CONFIRM, node_count, n, -1, PEER_DEFAULTPOWER, 1);

    msgsent_route++;
    bytesent_route += size;
  }
}

void handle_seqroutemsg(ADDRESS source_id, uint8_t *p)  // first-level handler for seq. routing msgs
{
  ROUTE_MSG msg;
  uint8_t size = unmarshal_routemsg(&msg, p);

DO_STACKTEST

  seq_route(source_id, &msg);
  msgrcv_route++;
  bytercv_route += size;
}
                                   
void handlesent_seqroutemsg(uint8_t msg_len, uint8_t *p, uint8_t send_success, uint8_t node_count,
  ADDRESS *nodes, uint8_t confirm_seq)                    // handler for failure to send of a seq. route
{
  ROUTE_MSG msg;

DO_STACKTEST

  if (send_success == 0)             // currently only failure is interesting here 
  {
    DBGOUT("Cannot send forward seq. route message!");
    if (node_count == 1)
    {
      unmarshal_routemsg(&msg, p);
      msg.cmd = CMD_SEQROUTEUNR;
      if (msg.is_repair && nodes[0] == msg.failed_node)
        msg.fail_count++;

      seq_route(nodes[0], &msg);
    } else {
      DBGERR("Seq. route message not sent to %d nodes!", node_count);
      pause_sim();
    }
  }
}

void handle_floodroutemsg(ADDRESS source_id, uint8_t *p)      // first-level handler for flood routing message
{
  ROUTE_MSG msg;
  uint8_t size = unmarshal_routemsg(&msg, p);

DO_STACKTEST

  flood_route(source_id, &msg);
  msgrcv_route++;
  bytercv_route += size;
}

void handle_basefloodmsg(ADDRESS source_id, uint8_t *p)  
{
  BASEFLOOD_MSG msg;
  uint8_t size = unmarshal_basefloodmsg(&msg, p);

DO_STACKTEST

  base_flood(source_id, &msg);
  msgrcv_route++;
  bytercv_route += size;
}

uint8_t checkaccept_floodroutemsg(ADDRESS source_id, uint8_t *p)
{
  ROUTE_MSG msg;

DO_STACKTEST

  unmarshal_routemsg_head(&msg, p);
  return checkaccept_floodroute(source_id, &msg);
}

uint8_t checkaccept_basefloodmsg(ADDRESS source_id, uint8_t *p)
{
  BASEFLOOD_MSG msg;

DO_STACKTEST

  unmarshal_basefloodmsg(&msg, p);
  return checkaccept_baseflood(source_id, &msg);
}
